#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/manual_control/manual_control.h>

#include <iostream>
#include <thread>
#include <termios.h>
#include <unistd.h>
#include <chrono>
#include <future>
#include <memory>
#include <algorithm>
#include <cstdint>
#include <iomanip>
#include <ros/ros.h>
#include "board_control/BoardControl.h"

#include "joystick/joystick.h"
#include <spdlog/spdlog.h>


using namespace mavsdk;
using namespace std;
using std::chrono::seconds;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;
ros::Subscriber board_control_sub_;
board_control::BoardControl board_control_;
void boardControlCallback(const board_control::BoardControl::ConstPtr &msg);
void test_subscribe(mavsdk::Telemetry& telemetry);
void manual_command_control();



int main(int argc, char** argv)
{
    ros::init(argc, argv, "board_control");
    ros::NodeHandle nh;
    
    board_control_sub_ = nh.subscribe("/uav1/board_control", 100, boardControlCallback); 
    // ---------------  连接飞控  --------------------
    Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
    std::string connection_url = "serial:///dev//ttyTHS0:115200"; // 默认连接UDP端口
    // 输出连接 URL 以便调试
    std::cout << "Attempting to connect using URL: " << connection_url << std::endl;
    ConnectionResult connection_result = mavsdk.add_any_connection(connection_url);

    if (connection_result != ConnectionResult::Success) {
        std::cout << "Connection failed: " << connection_result << std::endl;
        // 输出可能的连接 URL 以便调试
        std::cout << "Please check the connection URL format and ensure the device is available." << std::endl;
        return 1;
    }
    std::cout << "Connected Success...\n";

    auto system = mavsdk.first_autopilot(5.0);
    if (!system) {
        std::cerr << "Timed out waiting for system\n";
        return 1;
    }

    // Instantiate plugins.
    auto action = Action{system.value()};
    auto telemetry = Telemetry{system.value()};
    auto manual_control = ManualControl{system.value()};

    while (!telemetry.health_all_ok()) {
        std::cout << "Waiting for system to be ready\n";
        sleep_for(seconds(1));
    }
    std::cout << "System is ready\n";

    // test_set_rates(telemetry);
    test_subscribe(telemetry);

    //------------------------------------------------
    while (ros::ok()) {
        ros::spinOnce(); // 处理 ROS 事件
        manual_command_control();
    }

 
    std::cout << "end!\n";

    return 0;
}

void manual_command_control() {
    // --------- 解锁 ---------
    if (board_control_.armed == true) {
        std::cout << "Arming...\n";
        const Action::Result arm_result = action.arm();
        if (arm_result != Action::Result::Success) {
            std::cerr << "Arming failed: " << arm_result << '\n';
            return ;
        }
        for (unsigned i = 0; i < 10; ++i) {
            manual_control.set_manual_control_input(0.f, 0.f, 0.f, 0.f);
        }
    }

    // --------- 急停 ---------
    if (board_control_.kill == true) {

    }

}

void boardControlCallback(const board_control::BoardControl::ConstPtr &msg) {
    board_control_.armed = msg->armed;
    board_control_.kill = msg->kill;
    board_control_.stabilized = msg->stabilized;
    board_control_.position = msg->position;

    board_control_.axis.pitch = msg->axis.pitch;
    board_control_.axis.roll = msg->axis.roll;
    board_control_.axis.yaw = msg->axis.yaw;
    board_control_.axis.throttle = msg->axis.throttle;

    // std::cout << "--------------- 接收摇杆值 ------------------"<< std::endl;
    
    // std::cout << "按钮： 解锁 | 急停 | 模式[自稳] | 模式[定点] : " <<
    //             (board_control_.armed ? "true" : "false") << " | " << 
    //             (board_control_.kill ? "true" : "false") << " | " << 
    //             (board_control_.stabilized ? "true" : "false") << " | " << 
    //             (board_control_.position ? "true" : "false") << "\n";

    // std::cout << "摇杆： roll | pitch | yaw | throttle : " <<
    //             board_control_.axis.roll << " | " << 
    //             board_control_.axis.pitch << " | " << 
    //             board_control_.axis.yaw << " | " << 
    //             board_control_.axis.throttle << "\n";
    // std::cout << " " <<std::endl;
}

// void test_set_rates(mavsdk::Telemetry& _aTelemetry)
// {
//     {
//         const auto set_rate_result = _aTelemetry.set_rate_position_velocity_ned(1.0);
//         if (set_rate_result != Telemetry::Result::Success) {
//             std::cerr << "Setting set_rate_position_velocity_ned failed: " << set_rate_result << '\n';
//         }
//     }
//     {
//         const auto set_rate_result = _aTelemetry.set_rate_position(1.0);
//         if (set_rate_result != Telemetry::Result::Success) {
//             std::cerr << "Setting set_rate_position failed: " << set_rate_result << '\n';
//         }
//     }
//     // {
//     //     const auto set_rate_result = _aTelemetry.set_rate_home(1.0);
//     //     if (set_rate_result != Telemetry::Result::Success) {
//     //         std::cerr << "Setting set_rate_home failed: " << set_rate_result << '\n';
//     //     }
//     // }
//     // {
//     //     const auto set_rate_result = _aTelemetry.set_rate_in_air(1.0);
//     //     if (set_rate_result != Telemetry::Result::Success) {
//     //         std::cerr << "Setting set_rate_in_air failed: " << set_rate_result << '\n';
//     //     }
//     // }
//     // {
//     //     const auto set_rate_result = _aTelemetry.set_rate_landed_state(1.0);
//     //     if (set_rate_result != Telemetry::Result::Success) {
//     //         std::cerr << "Setting set_rate_landed_state failed: " << set_rate_result << '\n';
//     //     }
//     // }
//     // {
//     //     const auto set_rate_result = _aTelemetry.set_rate_velocity_ned(1.0);
//     //     if (set_rate_result != Telemetry::Result::Success) {
//     //         std::cerr << "Setting set_rate_velocity_ned failed: " << set_rate_result << '\n';
//     //     }
//     // }
//     {
//         const auto set_rate_result = _aTelemetry.set_rate_gps_info(1.0);
//         if (set_rate_result != Telemetry::Result::Success) {
//             std::cerr << "Setting set_rate_gps_info failed: " << set_rate_result << '\n';
//         }
//     }
//     {
//         const auto set_rate_result = _aTelemetry.set_rate_battery(1.0);
//         if (set_rate_result != Telemetry::Result::Success) {
//             std::cerr << "Setting set_rate_battery failed: " << set_rate_result << '\n';
//         }
//     }
//     // {
//     //     const auto set_rate_result = _aTelemetry.set_rate_distance_sensor(1.0);
//     //     if (set_rate_result != Telemetry::Result::Success) {
//     //         std::cerr << "Setting set_rate_distance_sensor failed: " << set_rate_result << '\n';
//     //     }
//     // }
   
// }

void test_subscribe(mavsdk::Telemetry& telemetry)
{
    //依赖GPS
    telemetry.subscribe_position([](Telemetry::Position Position) {
        std::cout << Position.relative_altitude_m << "\n";
    });

    telemetry.subscribe_position_velocity_ned([](Telemetry::PositionVelocityNed PositionNed) {
        std::cout << PositionNed.position << "\n";
    });

    telemetry.subscribe_position_velocity_ned([](Telemetry::PositionVelocityNed VelocityNed) {
        std::cout << VelocityNed.velocity << "\n";
    });

    // telemetry.subscribe_home([](Telemetry::Position Home) {
    //     std::cout << Home << "\n";
    // });

    // telemetry.subscribe_in_air([](bool InAir) {
    //     std::cout << InAir << "\n";
    // });

    // telemetry.subscribe_status_text([](Telemetry::StatusText StatusText) {
    //     std::cout << StatusText << "\n";
    // });

    // telemetry.subscribe_armed([](bool Armed) {
    //     std::cout << Armed << "\n";
    // });

    // telemetry.subscribe_attitude_quaternion([](Telemetry::Quaternion Att_Quat) {
    //     std::cout << Att_Quat << "\n";
    // });

    // telemetry.subscribe_attitude_euler([](Telemetry::EulerAngle Att_Euler) {
    //     std::cout << Att_Euler << "\n";
    // });

    // telemetry.subscribe_attitude_angular_velocity_body([](Telemetry::AngularVelocityBody Att_Angular_Velocity) {
    //     std::cout << Att_Angular_Velocity << "\n";
    // });

    // telemetry.subscribe_attitude_euler([](Telemetry::EulerAngle Att_Euler) {
    //     std::cout << Att_Euler << "\n";
    // });

    //依赖GPS
    // telemetry.subscribe_velocity_ned([](Telemetry::VelocityNed Velocity_NED) {
    //     std::cout << Velocity_NED << "\n";
    // });

    // telemetry.subscribe_imu([](Telemetry::Imu IMU) {
    //     std::cout << IMU << "\n";
    // });

    // telemetry.subscribe_scaled_imu([](Telemetry::Imu Scaled_IMU) {
    //     std::cout << Scaled_IMU << "\n";
    // });

    // telemetry.subscribe_raw_imu([](Telemetry::Imu Raw_IMU) {
    //     std::cout << Raw_IMU << "\n";
    // });

    telemetry.subscribe_gps_info([](Telemetry::GpsInfo GPS_Info) {
        std::cout << GPS_Info.num_satellites << "\n";
    });

    // telemetry.subscribe_raw_gps([](Telemetry::RawGps Raw_GPS_Info) {
    //     std::cout << Raw_GPS_Info << "\n";
    // });

    telemetry.subscribe_battery([](Telemetry::Battery Battery) {
        std::cout << Battery.voltage_v << "\n";
    });

    telemetry.subscribe_flight_mode([](Telemetry::FlightMode Flight_Mode) {
        std::cout << Flight_Mode << "\n";
    });

    // telemetry.subscribe_health([](Telemetry::Health Health) {
    //     std::cout << Health << "\n";
    // });

    // telemetry.subscribe_health_all_ok([](bool Health_All_Ok) {
    //     std::cout << Health_All_Ok << "\n";
    // });

    // telemetry.subscribe_landed_state([](Telemetry::LandedState Landed_State) {
    //     std::cout << Landed_State << "\n";
    // });

    // telemetry.subscribe_distance_sensor([](Telemetry::DistanceSensor Distance_sensor) {
    //     std::cout << Distance_sensor << " m\n";
    // });
}

